#include <sys/time.h>
#include "Devices.h"
#include <cstdlib> 
#include <ctime>
#include <stdio.h>
#include <fstream>
#include <libpowerbutton.h>

#include "main.h"
#define FORWARD 90

extern int flash_count;
extern int freq_stop;

extern int* buffer;

extern bool foundLight;

int main(int argc, char **argv)
{
    float freq;
    srand(time(NULL));
    Mot = new DCMotor();
    Mot->Open(-1);

    IB = new InterfaceBoard();
    Ser = new Servo();
    // Open connection to the board. If serial number is not known use -1
    IB->Open(-1); 
    Ser->Open(-1);
	Ser->SetPosition(0);
    power_button_reset();

// ----- wait for power button to start ----- //
    while(power_button_get_value()<1){sleep(1);}
    power_button_reset();

	printf("Waking up.. wait a sec...\n");

	Ser->SetPosition(0);
	wait(100);
	Ser->SetPosition(FORWARD);
	wait(500);

/*
	for (int i = 1; i < 5; i++)
	{
		
		power_button_reset();
	    Ser->SetPosition(FORWARD);
		wait(200);		
		sonarScan();
		move(100,100);
		lookForBlack();
		irScan();
		lookForSwitch();
		lookForLight();
		int j = 0;

		while(!foundLight && j < 4){
 		    Ser->SetPosition(FORWARD);
		    goBack();
		    move(0,0);
		    irScan();
		    lookForSwitch();
		    lookForLight();
		    j++;
		}

		
		if(foundLight){checkFreq();}
		//freq = -1.0;
		freq = analyzeFreq();
		printf("Freq: %f ±2Hz\n", freq);
		if(freq > -1){idRS(freq);}

		
		while(freq == -1)
		{
			printf("Failed to detect frequency\n");
			Ser->SetPosition(FORWARD);
			goBack();
			irScan();
			lookForSwitch();
			lookForLight();
			if(foundLight){checkFreq();}
			freq = analyzeFreq();
			printf("Freq: %f ±2Hz\n", freq);
			if(freq > -1){idRS(freq);}	
		}

		//facing away from RS
		Ser->SetPosition(FORWARD);
		if(power_button_get_value()>0){break;}	
		power_button_reset();
		
	}
*/
// ------------------ new code -------------------- //
	int idrs = -1;
	bool RSCompleted[5] = {false,false,false,false,false};
	bool all_completed = false;
	int tries = 0;
	//sonarScan();
	while(not all_completed)
	{
		if(idrs == -1)
		{
			move(0,0);
			//sonarScan();
		}
		freq = 0.0;
		Ser->SetPosition(FORWARD);
		wait(100);		
		move(100,100);
		lookForBlack();
		irScan();
		lookForSwitch();
		lookForLight();
		int tries = 0;
		
		while(not foundLight && tries < 3){
 		    Ser->SetPosition(FORWARD);
		    goBack();
		    move(0,0);
		    irScan();
		    lookForSwitch();
		    lookForLight();
		    tries++;
		}
		if(tries == 3){
			sonarScan();
			move(100,100);
			lookForBlack();
			irScan();
			lookForSwitch();
			lookForLight();
		}
		tries = 0;
		
		if(foundLight){checkFreq();}

		freq = analyzeFreq();
		printf("Freq: %f ±2Hz\n", freq);

		if(freq > -1){
			idrs = idRS(freq);
			RSCompleted[idrs-1] = true;
		}


		while(freq == -1)
		{
			printf("Failed to detect frequency\n");
			Ser->SetPosition(FORWARD);
			goBack();
			irScan();
			lookForSwitch();
			lookForLight();
			if(foundLight){checkFreq();}
			freq = analyzeFreq();
			printf("Freq: %f ±2Hz\n", freq);
			if(freq > -1){
				idrs = idRS(freq);
				RSCompleted[idrs-1] = true;
			}	
		}
		
		Ser->SetPosition(0);
		Ser->SetPosition(FORWARD);

		// check if all sites have been visited
		bool tmp = true;
		for(int j = 0; j < 5; j++)
		{
			tmp = (tmp and RSCompleted[j]);
		}
		if(tmp == true) {all_completed = true;}
		printf("All_completed = %d\nidrs = %d\n", all_completed, idrs);
	
		// exit strategy for resource site - use to get to next site
		if(not all_completed){
			exitRS(idrs);
			printf("did exitRS");
		}
		else
		{
			// Finished !!!!
			move(0,0);
			wait(500);
			move(-100,-100);
			wait(1000);
			move(-100,100);
			Ser->SetPosition(0);
			wait(1000);
			Ser->SetPosition(FORWARD);
			wait(1000);
			Ser->SetPosition(FORWARD * 2);
			wait(1000);	
			Ser->SetPosition(0);
			wait(1000);
			move(0,0);
			break;
		}
		
	}

// ------------------ new code -------------------- //

    freq_stop = 0;
    free(buffer);


    // Close motor
    Mot->Close();
    // Dispose of object
    delete Mot;
    
    Ser->Close();
    delete Ser;

    // Close IB
    IB->Close();
    // Dispose of object
    delete IB;
    return 0;
}
